#include "../inc/precompiled.h"

namespace cg
{
	const Mat4 MAT4_ZERO( Vec4::ZERO, Vec4::ZERO, Vec4::ZERO, Vec4::ZERO );

	// Creates a 4x4 rotation matrix, takes radians NOT degrees
	// inspiration taken from idLib :-)
	void RotationMatrix( Mat4 &m, float angle, const Vec3 &axis )
	{
		float x = axis.x;
		float y = axis.y;
		float z = axis.z;

		float mag, s, c;
		float xx, yy, zz, xy, yz, zx, xs, ys, zs, one_c;

		s = float( sin( angle ) );
		c = float( cos( angle ) );

		mag = float( sqrt( x*x + y*y + z*z ) );

		// Identity matrix
		if (mag == 0.0f) {
			LoadIdentity(m);
			return;
		}

		// Rotation matrix is normalized
		x /= mag;
		y /= mag;
		z /= mag;

		float*p = toFloat( m );

		#define M(row,col)  p[col*4+row]

		xx = x * x;
		yy = y * y;
		zz = z * z;
		xy = x * y;
		yz = y * z;
		zx = z * x;
		xs = x * s;
		ys = y * s;
		zs = z * s;
		one_c = 1.0f - c;

		M(0,0) = ( one_c * xx ) + c;
		M(0,1) = ( one_c * xy ) - zs;
		M(0,2) = ( one_c * zx ) + ys;
		M(0,3) = 0.0f;

		M(1,0) = ( one_c * xy ) + zs;
		M(1,1) = ( one_c * yy ) + c;
		M(1,2) = ( one_c * yz ) - xs;
		M(1,3) = 0.0f;

		M(2,0) = ( one_c * zx ) - ys;
		M(2,1) = ( one_c * yz ) + xs;
		M(2,2) = ( one_c * zz ) + c;
		M(2,3) = 0.0f;

		M(3,0) = 0.0f;
		M(3,1) = 0.0f;
		M(3,2) = 0.0f;
		M(3,3) = 1.0f;

		#undef M
	}

	CG_INLINE char* toChar( const Mat4& m )
	{
		char* str = (char*)malloc( sizeof(char) * 200);

		const float* p = toFloat( m );
				
		std::stringstream ss;
		for( int i = 0; i < 16; i++ ) {
			int col = i / 4;
			int row = i % 4;
			ss << p[ row * 4 + col ] << " ";
			//ss << p[ i ] << " ";
			if ( row == 3 )
				ss << std::endl;
		}

		strcpy( str, ss.str().c_str() );
				
		return str;		
	}
}